#include "zf_common_headfile.h"
#include "my_car.h"

int16 limit_ab(int16 x, int16 a, int16 b)
{
    if(x<a) x = a;
    if(x>b) x = b;
    return x;
}


void my_pwm_init(void)
{

  pwm_init(TIM5_PWM_CH1_A0, 13000,0);
  gpio_init(A2, GPO, GPIO_HIGH, GPO_PUSH_PULL);
  gpio_init(C0, GPO, GPIO_HIGH, GPO_PUSH_PULL);

  pwm_init(TIM5_PWM_CH4_A3, 13000,0);
  gpio_init(A1, GPO, GPIO_HIGH, GPO_PUSH_PULL);
  gpio_init(C2, GPO, GPIO_HIGH, GPO_PUSH_PULL);
  

  pwm_init(TIM4_PWM_CH2_B7, 13000,0);
  gpio_init(B6, GPO, GPIO_HIGH, GPO_PUSH_PULL);
  gpio_init(B4, GPO, GPIO_HIGH, GPO_PUSH_PULL);

  pwm_init(TIM4_PWM_CH4_B9, 13000,0);
  gpio_init(B5, GPO, GPIO_HIGH, GPO_PUSH_PULL);
  gpio_init(B8, GPO, GPIO_HIGH, GPO_PUSH_PULL);  
}


void my_motor(int16 powerl_front,int16 powerr_front,int16 powerl_after,int16 powerr_after)
{

  powerl_front = limit_ab(powerl_front,-990,990);
  powerr_front = limit_ab(powerr_front,-990,990);
  powerl_after = limit_ab(powerl_after,-990,990);
  powerr_after = limit_ab(powerr_after,-990,990);

  if(powerl_front > 0)
  {
    pwm_set_duty(TIM5_PWM_CH1_A0,powerl_front);
    gpio_set_level(A2, GPIO_LOW);
    gpio_set_level(C0, GPIO_HIGH);
  }
  else 
  {
    pwm_set_duty(TIM5_PWM_CH1_A0,-powerl_front);
    gpio_set_level(A2, GPIO_HIGH);
    gpio_set_level(C0, GPIO_LOW);
  }

  if(powerr_front > 0)
  {
    pwm_set_duty(TIM5_PWM_CH4_A3,powerr_front);
    gpio_set_level(A1, GPIO_HIGH);
    gpio_set_level(C2, GPIO_LOW);
  }
  else 
  {
    pwm_set_duty(TIM5_PWM_CH4_A3,-powerr_front);
    gpio_set_level(A1, GPIO_LOW);
    gpio_set_level(C2, GPIO_HIGH);
  }
  

  if(powerl_after > 0)
  {
    pwm_set_duty(TIM4_PWM_CH2_B7,powerl_after);
    gpio_set_level(B6, GPIO_LOW);
    gpio_set_level(B4, GPIO_HIGH);
  }
  else 
  {
    pwm_set_duty(TIM4_PWM_CH2_B7,-powerl_after);
    gpio_set_level(B6, GPIO_HIGH);
    gpio_set_level(B4, GPIO_LOW);
  }

  if(powerr_after > 0)
  {
    pwm_set_duty(TIM4_PWM_CH4_B9,powerr_after);
    gpio_set_level(B5, GPIO_LOW);
    gpio_set_level(B8, GPIO_HIGH);
  }
  else 
  {
    pwm_set_duty(TIM4_PWM_CH4_B9,-powerr_after);
    gpio_set_level(B5, GPIO_HIGH);
    gpio_set_level(B8, GPIO_LOW);
  }
}

